#ifndef __PID_H__
#define __PID_H__

namespace control_test {
    class PID {
        public:
            PID(int h_, int K_, int Ti_, int Td_, int b_=1);
            int Update(int target, int current);


        private:
            // Parameters of the PID
            static const int umin = -1023;
            static const int umax = 1023;
            static const int N = 10;       // LPF parameter
            int h;                  // Sampling time
            int K;                  // Proportional gain
            int Ti;                 // Integration time
            int Td;                 // Derivation time
            int b;                  // Additional parameter for the prop. part
            int ad;
            int bd;
            int bi;
            int a0;
            // Internal variables
            int up;                 // Proportional part
            int ui;                 // Integral part
            int ud;                 // Derivative part
            int prev_output;        // Value of the output of the system at t-1
            int v;                  // Control signal
            int u;                  // Saturated control signal

    };
};

#endif // __PID_H__
